%%This file is for the simulation of single case and plot the six-combined-figures appered in the paper.
warning('off'); close all; 
% filed limits [x_min x_max y_min y_max z_min z_max]
field_limits = [0 100 0 100 0 20];
state_dim = 3;                                            % state dimensions for each robot
robot = [ rand(r_num,1)*field_limits(2), rand(r_num,1)*field_limits(4), rand(r_num,1)*field_limits(6) ];
savepath = ['Results/singleDensityCase/', num2str(r_num),'robot/'];
if ~isfolder(savepath)
    mkdir(savepath);
end

% plot of trajectory
robot_scatter_size = 10;

% discretizing the x-y plane, 
discretization_num = 50; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits(1),field_limits(2),discretization_num);
y_space = linspace(field_limits(3),field_limits(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];

% density calculation
centers = [50 50]; c_index = 1; duration = 500;
d_center = centers(c_index,:);
R = densityFunc(d_center,points_xy);
%integral = sum(R*mesh_size^2,'all')
h = robot(:,end); sensor_para = 20;
[r,dr_dh] = coverageModel(h,sensor_para,'reciprocal');

% variables preparation 
robot_init = robot;     robot_his = cell(100,1);     robot_his{1} = robot;

for k = 1:duration
    for i = 1:r_num
        % refine the robot set of multual communication
        s_i = robot(i,:); 
        rel_dis = vecnorm(s_i(1:2) - robot(:,1:2),2,2); 
        r = coverageModel(robot(:,end),sensor_para,'reciprocal');
        robot_i = robot; r1 = rel_dis < r + r(i); r1(rel_dis == 0) = 1;
        robot_i = robot_i .* r1;
        [r,dr_dh] = coverageModel(robot_i(:,end),sensor_para,'reciprocal'); r = r.*r1; dr_dh = dr_dh.*r1;
        % compute the gradient
        delta_u_i = computeGradient(i,robot_i,r,dr_dh,points_xy,mesh_size,R,d_center);
        direction = delta_u_i/norm(delta_u_i,2);

        robot1(i,:) = robot(i,:) + 0.1* direction';
    end
    k/duration;
    robot_his{k+1} = robot1;        robot = robot1; robot(:,3) = abs(robot(:,3));
    h = robot(:,end); [r,dr_dh]= coverageModel(h,sensor_para,'reciprocal');    
end    
t = datetime('now');
save([savepath,num2str(t.Minute),num2str(t.Second),'robotPos.mat'], 'robot_his');
[X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
h = figure('Name','CoverageAreaAfterOptimization','visible','off'); hold on; grid on;
surf(X,Y,Z); shading interp;colormap default; colorbar; 

saveas(h,[savepath,num2str(t.Minute),num2str(t.Second),'.jpg']);

%% plotTrajectories
clc;clear;close all;
field_limits = [0 100 0 100 0 20];
r_num = 4;
dataname = '223.8583';
savepath = ['Results/singleDensityCase/', num2str(r_num),'robot/'];

load([savepath,dataname,'robotPos.mat'])
robot_his = cell2mat(robot_his);
robot_int = robot_his(1:r_num,:);
robot_end = robot_his(end-r_num+1:end,:);

%DensityDistribution
f6 = figure('Name','DensityDistribution')
field_limits1 = [field_limits(1:4) 0 1];
discretization_num = 500; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits1(1),field_limits1(2),discretization_num);
y_space = linspace(field_limits1(3),field_limits1(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];
d_centers = [50 50]; 
R = densityFunc(d_centers,points_xy);
surf(X_points,Y_points,R);shading interp; colorbar; hold on;
view(2);caxis manual; caxis([0 1]); axis(field_limits1)
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f6,'DensityDistribution.eps','epsc');

% Trajectories in 3D
rang = cell(r_num,1);
rang{1}= 1:r_num:length(robot_his);
rang{2}= 2:r_num:length(robot_his);
rang{3}= 3:r_num:length(robot_his);
rang{4}= 4:r_num:length(robot_his);
f4 = figure('Name','PositionsIn2D'); hold on; grid on;
scattersize1 = 30; scattersize2 = 60; c = ['r','g','b','k'];
for i = 1:r_num
    scatter3(robot_int(i,1),robot_int(i,2),abs(robot_int(i,3)),scattersize1,'o',c(i),'filled');
    scatter3(robot_end(i,1),robot_end(i,2),abs(robot_end(i,3)),scattersize2,'v',c(i),'filled');
    scatter3(robot_his(rang{i},1),robot_his(rang{i},2),abs(robot_his(rang{i},3)),1,'.',c(i));
end
axis(field_limits);view(79,49);
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f4,'Trajectories3D.eps','epsc');

% positions in 3D
f1 = figure('Name','posotionComparison'); hold on; view(79,49);;grid on;
scattersize1 = 20; scattersize2 = 40; c = ['r','g','b','k'];
for i = 1:r_num
    scatter3(robot_int(i,1),robot_int(i,2),abs(robot_int(i,3)),scattersize1,'o',c(i),'filled')
    scatter3(robot_end(i,1),robot_end(i,2),abs(robot_end(i,3)),scattersize2,'v',c(i),'filled')
end
axis(field_limits);
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f1,'Positions3D.eps','epsc');

% positions in 2D
f5 = figure('Name','PositionsIn2D'); hold on; view(2);grid off;
scattersize1 = 20; scattersize2 = 40; c = ['r','g','b','k'];
for i = 1:r_num
    scatter3(robot_int(i,1),robot_int(i,2),abs(robot_int(i,3)),scattersize1,'o',c(i),'filled')
    scatter3(robot_end(i,1),robot_end(i,2),abs(robot_end(i,3)),scattersize2,'v',c(i),'filled')
end
axis(field_limits);
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f5,'Positions2D.eps','epsc');

%PDOP initial
f2 = figure('Name','PDOPdistributionInitial'); 
[X,Y,Z] = computeMeshforPlot(robot_int,field_limits,200);
surf(X,Y,Z); shading interp; colorbar;view(2);
caxis manual
caxis([0 1]);

ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f2,'PDOPinitial.eps','epsc');

% PDOP after
f3 = figure('Name','PDOPdistributionLocal'); 
[X,Y,Z] = computeMeshforPlot(robot_end,field_limits,200);
surf(X,Y,Z); shading interp; colorbar; view(2);
caxis manual
caxis([0 1]);

ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f3,'PDOPend.eps','epsc');


%%
% time = length(robot_his)/r_num;
% for t = 1:time
%    fplot(@(x) sin(x*50/t),[0,2*pi]);  % plot
%    ylim([-1,1]);                      % guarantee consistent height
%    F(t) = getframe;                   % capture it
% end
% 
% writerObj = VideoWriter('test2.avi');
% open(writerObj);
% writeVideo(writerObj, F)
% close(writerObj);